#include "Matrix.h"
#include <cassert>

namespace uav
{
namespace math
{

const Matrix Matrix::Identity = Matrix(
	Fixed::One,  Fixed::Zero, Fixed::Zero,
	Fixed::Zero, Fixed::One,  Fixed::Zero,
	Fixed::Zero, Fixed::Zero, Fixed::One
);

Fixed Matrix::determinant() const
{
	return m11 * cofactor11()
	     + m12 * cofactor12()
	     + m13 * cofactor13();
}

Matrix Matrix::inverse() const
{
	Matrix res(cofactor11(), cofactor21(), cofactor31(),
	           cofactor12(), cofactor22(), cofactor32(),
	           cofactor13(), cofactor23(), cofactor33());
	Fixed det = (m11 * res.m11) + (m12 * res.m21) + (m13 * res.m31);
	assert(det != Fixed::Zero);
	return res * (Fixed::One / det);
}

Matrix Matrix::operator*(const Matrix &m) const
{
	return Matrix(
		m11 * m.m11 + m12 * m.m21 + m13 * m.m31,
		m11 * m.m12 + m12 * m.m22 + m13 * m.m32,
		m11 * m.m13 + m12 * m.m23 + m13 * m.m33,

		m21 * m.m11 + m22 * m.m21 + m23 * m.m31,
		m21 * m.m12 + m22 * m.m22 + m23 * m.m32,
		m21 * m.m13 + m22 * m.m23 + m23 * m.m33,

		m31 * m.m11 + m32 * m.m21 + m33 * m.m31,
		m31 * m.m12 + m32 * m.m22 + m33 * m.m32,
		m31 * m.m13 + m32 * m.m23 + m33 * m.m33
	);
}

Matrix& Matrix::operator*=(const Matrix &m)
{
	Fixed t1, t2;

	t1  = m11 * m.m11 + m12 * m.m21 + m13 * m.m31;
	t2  = m11 * m.m12 + m12 * m.m22 + m13 * m.m32;
	m13 = m11 * m.m13 + m12 * m.m23 + m13 * m.m33;
	m11 = t1; m12 = t2;

	t1  = m21 * m.m11 + m22 * m.m21 + m23 * m.m31;
	t2  = m21 * m.m12 + m22 * m.m22 + m23 * m.m32;
	m23 = m21 * m.m13 + m22 * m.m23 + m23 * m.m33;
	m21 = t1; m22 = t2;

	t1  = m31 * m.m11 + m32 * m.m21 + m33 * m.m31;
	t2  = m31 * m.m12 + m32 * m.m22 + m33 * m.m32;
	m33 = m31 * m.m13 + m32 * m.m23 + m33 * m.m33;
	m31 = t1; m32 = t2;

	return *this;
}

Matrix& Matrix::operator+=(const Matrix &m)
{
	m11 += m.m11; m12 += m.m12; m13 += m.m13;
	m21 += m.m21; m22 += m.m22; m23 += m.m23;
	m31 += m.m31; m32 += m.m32; m33 += m.m33;
	return *this;
}

Matrix& Matrix::operator-=(const Matrix &m)
{
	m11 -= m.m11; m12 -= m.m12; m13 -= m.m13;
	m21 -= m.m21; m22 -= m.m22; m23 -= m.m23;
	m31 -= m.m31; m32 -= m.m32; m33 -= m.m33;
	return *this;
}

Matrix& Matrix::operator*=(const Fixed &s)
{
	m11 *= s; m12 *= s; m13 *= s;
	m21 *= s; m22 *= s; m23 *= s;
	m31 *= s; m32 *= s; m33 *= s;
	return *this;
}

Matrix& Matrix::operator/=(const Fixed &s)
{
	m11 /= s; m12 /= s; m13 /= s;
	m21 /= s; m22 /= s; m23 /= s;
	m31 /= s; m32 /= s; m33 /= s;
	return *this;
}

}}
